
#include "User_Program.h"
#include "Ano_ProgramCtrl_User.h"

#include "Drv_usart.h"
#include "Drv_led.h"


//用户任务顶层


//uint8_t Flag;
///*
//        |+X 
//        |
//        |
//+Y------------- -Y
//        |
//        |
//        |-X
//        
uint8_t takeoff_flag;
FollowManager_t FollowManager;


int16_t coordinates[4][24]=
{
	80,150,140,0xa3
};
///////////////航点列表/////////////
Point_t A0;
Point_t A1;
Point_t A2;
Point_t A3;
Point_t A4;
Point_t A5;
Point_t A6;
Point_t A7;
Point_t A8;
Point_t A9;
///////////////航点列表////////////
void Position_Init()
{
A0.x=96;
A0.y=-50;
A0.z=110;
A0.code=0xA0;

A1.x=225;
A1.y=-63;
A1.z=110;
A1.code=0xA1;

A2.x=225;
A2.y=-95;
A2.z=110;
A2.code=0xA2;

A3.x=223;
A3.y=-171;
A3.z=110;
A3.code=0xA3;

A4.x=221;
A4.y=-263;
A4.z=110;
A4.code=0xA4;

A5.x=133;
A5.y=-263;
A5.z=110;
A5.code=0xA5;

A6.x=77;
A6.y=-263;
A6.z=110;
A6.code=0xA6;

A7.x=133;
A7.y=-196;
A7.z=110;
A7.code=0xA7;

A8.x=121;
A8.y=-142;
A8.z=110;
A8.code=0xA8;

A9.x=43;
A9.y=-14;
A9.z=110;
A9.code=0xA9;
}

bool ActionHoldChange(int16_t HoldTime, FSMList_t NextAction)
{
    static bool Enter = true;
    static uint16_t CountDown = 0;

    if (Enter)
    {
        CountDown = HoldTime;
        Enter = false;
    }
    else
    {
        CountDown--;
    }

    if (CountDown == 0)
    {
        Enter = true;
        FollowManager.ActionList = NextAction;
			return 1;
    }
		return 0;
}

bool ActionDelay(int16_t HoldTime)
{
    static bool Enter = true;
    static uint16_t CountDown = 0;

    if (Enter)
    {
        CountDown = HoldTime;
        Enter = false;
    }
    else
    {
        CountDown--;
    }

    if (CountDown == 0)
    {
        Enter = true;
		return 1;
    }
		return 0;

}

bool Go_Position(Point_t Pos)
{
if(sdk_pos_set(Pos.x,Pos.y)&&sdk_alititude_set(Pos.z)){return 1;}
    return 0;
}

void Goto_Action(FSMList_t GoAction)
{
FollowManager.ActionList=GoAction;
}


uint8_t ActionIRQ_Flag=0;//任务中断标志位
/*任务中断请求接口函数 */
void ActionIRQ_Request(FSMList_t Action_Req)
{
ActionIRQ_Flag=1;
FollowManager.Action_IRQ=Action_Req;
}
/*任务中断请求处理Handler */
void SoftActionIRQ()
{
	if(ActionIRQ_Flag){
FollowManager.Action_Last= FollowManager.ActionList;ActionIRQ_Flag=0;
FollowManager.ActionList= FollowManager.Action_IRQ;
	}
}
/*任务中断退出恢复原任务函数*/
void Action_Resume()
{
	FollowManager.ActionList= FollowManager.Action_Last;
}


////////////////////////////////////////////////////////////////////
//10ms一次循环 100即为1s
u32 test_task_time;
void Test_ProgramCtrl(float dT_s,s16 *CH_N)
{
	//判断按键下拨
//	if(takeoff_flag==0){
		if(CH_N[7]>400) 
		{
			/* 按键下拨开始计时 */
			test_task_time += dT_s;  
			if(test_task_time == 500) 
			{
				sdk_yaw_set(0);
				sdk_takeoff(100);//一键起飞
			}
			else if(test_task_time >= 4000 && test_task_time < 10000)//&& test_task_time <= 6000
			{
				sdk_alititude_set(180);
			}
			else if(test_task_time >= 10000 )//&& test_task_time <= 6000
			{
				sdk_alititude_set(100);
			}
		}
		else if(CH_N[7]<-400){
			test_task_time = 0;
			sdk_land();
		}
//	}
}
//10ms一次循环 100即为1s
void UpdateStatus(float dT_s,s16 *CH_N)
{
	// 按键下拨后判断按键 
if(takeoff_flag==1){
if(CH_N[7]<-400){
sdk_land();Goto_Action(ActionWaitting);takeoff_flag=2;
}
}
//判断按键下拨 0.1秒后起飞 进入程控模式
if(takeoff_flag==0){
if(CH_N[7]>400){Goto_Action(ActionCountdown);takeoff_flag=1;}
}

//if(CH_N[9]<-400){flag.unlock_sta=0;}
////遥控器锁定以及遥控器开启程序控制//

SoftActionIRQ();//任务中断线程

switch(FollowManager.ActionList)
{
	case(ActionWaitting):
{
	
	break;
}
	case(ActionCountdown):
{
	ledFlash(0.01,BIT_RLED,40,40);
	flag.action_activated=1;
	if(ActionHoldChange(10,ActionTakeOff))Uart5_Send ("TKOF!",8);
	break;
}
case(ActionTakeOff):
{
	//一键起飞
	sdk_takeoff(100);
	Goto_Action(ActionHoverStartPoint);
break;
}
case ActionHoverStartPoint:
{
sdk_pos_set(0,0);
ActionHoldChange(1200,Action_A0);
break;
}
case Action_A0:
{
Go_Position(A0);
ActionHoldChange(800,Action_A1);
break;
}
case Action_A1:
{
Go_Position(A1);
ActionHoldChange(800,Action_A2);
break;
}
case Action_A2:
{
Go_Position(A2);
ActionHoldChange(800,Action_A3);
break;
}
case Action_A3:
{
Go_Position(A3);
ActionHoldChange(800,Action_A4);
break;
}
case Action_A4:
{
Go_Position(A4);
ActionHoldChange(800,Action_A5);
break;
}
case Action_A5:
{
Go_Position(A5);
ActionHoldChange(800,Action_A6);
break;
}
case Action_A6:
{
Go_Position(A6);
ActionHoldChange(800,Action_A7);
break;
}
case Action_A7:
{
Go_Position(A7);
ActionHoldChange(800,Action_A8);
break;
}
case Action_A8:
{
Go_Position(A8);
ActionHoldChange(800,Action_A9);
break;
}
case Action_A9:
{
Go_Position(A9);
ActionHoldChange(800,Action_Land);
break;
}


case(Action_Land):
{
	if(ActionHoldChange(100,ActionTakeOff))
	{
	sdk_land();
	Goto_Action(ActionWaitting);
	}
	break;
}


}


		
}


